﻿using Microsoft.Xna.Framework;
using System;

namespace xEngine.Maths
{
    public static class XMath
    {
        #region Functions

        public static bool CircleRectangleIntersection(Circle circle, IRectangle rect)
        {
            Vector2 closest = ClosestRectanglePointToPoint(rect, circle.Center);

            // Could use this, but Vector2.Distance uses sqrt which is more performance heavy than following approach
            // and considering dozens of thousands of calls to this methods each frame - it adds up to nociable frametime drop
            // 
            // return Vector2.Distance(closest, Convert.V3ToV2(circle.Center)) <= circle.Radius;

            float distanceX = circle.Center.X - closest.X;
            float distanceZ = circle.Center.Z - closest.Y;

            return ( (distanceX * distanceX) + (distanceZ * distanceZ) ) <= (circle.Radius * circle.Radius);
        }

        public static Vector2 ClosestRectanglePointToPoint(IRectangle rect, Vector3 point)
        {
            return ClosestRectanglePointToPoint(rect, Utils.Convert.V3ToV2xz(point));
        }
        public static Vector2 ClosestRectanglePointToPoint(IRectangle rect, Vector2 point)
        {
            return new Vector2(MathHelper.Clamp(point.X, rect.Left, rect.Right),
                               MathHelper.Clamp(point.Y, rect.Top, rect.Bottom));
        }
        public static Vector3 ClosestBoxPointToPoint(BoundingBox box, Vector3 point)
        {
            return new Vector3(MathHelper.Clamp(point.X, box.Min.X, box.Max.X),
                               MathHelper.Clamp(point.Y, box.Min.Y, box.Max.Y),
                               MathHelper.Clamp(point.Z, box.Min.Z, box.Max.Z));
        }

        public static bool CheckDistance(Vector3 a, Vector3 b, float d)
        {
            float dX = b.X - a.X;
            float dY = b.Y - a.Y;
            float dZ = b.Z - a.Z;

            return (dX * dX) + (dY * dY) + (dZ * dZ) <= (d * d);
        }

        public static Matrix RotationXZFromUp(Vector3 up)
        {
            Vector3 angles = AnglesFromUp(up);
            return Matrix.CreateRotationX(angles.X) * Matrix.CreateRotationZ(angles.Z);
        }
        public static Matrix RotationXYFromForward(Vector3 forward)
        {
            Vector3 angles = AnglesFromForward(forward);
            return Matrix.CreateRotationX(angles.X) * Matrix.CreateRotationY(angles.Y);
        }
        public static Matrix RotationFromUpTarget(Vector3 up, Vector3 target)
        {
            Matrix m = Matrix.Identity;
            m.Up = Vector3.Normalize(up);
            m.Forward = Vector3.Normalize(target);
            m.Right = Vector3.Normalize(Vector3.Cross(target, up));
            return m;
        }
        public static Vector3 AnglesFromForward(Vector3 forward)
        {
            return new Vector3((float)Math.Asin(forward.Y), (float)-Math.Atan2(forward.X, -forward.Z), 0);
        }
        public static Vector3 AnglesFromUp(Vector3 up)
        {
            Vector3 v1 = new Vector3(0, up.Y, up.Z),
                    v2 = new Vector3(up.X, up.Y, 0);

            float rotx = (!v1.Equals(Vector3.Zero) ? (float)Math.Acos(Vector3.Dot(Vector3.Up, Vector3.Normalize(v1))) : 0);
            float rotz = (!v2.Equals(Vector3.Zero) ? -(float)Math.Acos(Vector3.Dot(Vector3.Up, Vector3.Normalize(v2))) : 0);

            return new Vector3(rotx, 0, rotz);
        }
        public static Vector3 AnglesFromMatrix(Matrix m)
        {
            float rotx1 = (float)Math.Acos(m.M22);
            float roty1 = (float)Math.Acos(m.M11);

            float rotx2 = (float)Math.Asin(m.M23);
            float roty2 = -(float)Math.Asin(m.M13);

            if (roty2 < 0)
                roty1 = MathHelper.TwoPi - roty1;

            float check = roty1 - MathHelper.PiOver2;

            if (rotx2 < 0)
                rotx1 = -rotx1;

            if (check > 0 && check < MathHelper.Pi)
                rotx1 = -rotx1;

            return new Vector3(rotx1, roty1, 0);
        }

        #endregion

        #region Enums
        #endregion
    }
}
